#include <iostream>
#include <fstream>
#include <sstream>
#include <stdio.h>
#include <cmath>
#include <math.h>
#include <algorithm>
#include <string>
#include <stdlib.h>
#include "wy.h"
#include <QFile>
#include <QDebug>

using namespace std;


#define MRD(x)  (int(roundf(x)))


void Rel2Abs(float delta_P[6], float pr_tip[6], float p_tool_d[6])
{
    p_tool_d[0] = pr_tip[0] - delta_P[1] * (cos(pr_tip[3])*sin(pr_tip[5]) - cos(pr_tip[5])*sin(pr_tip[3])*sin(pr_tip[4])) + delta_P[2] * (sin(pr_tip[3])*sin(pr_tip[5]) + cos(pr_tip[3])*cos(pr_tip[5])*sin(pr_tip[4])) + delta_P[0] * cos(pr_tip[4])*cos(pr_tip[5]);
    p_tool_d[1] = pr_tip[1] + delta_P[1] * (cos(pr_tip[3])*cos(pr_tip[5]) + sin(pr_tip[3])*sin(pr_tip[4])*sin(pr_tip[5])) - delta_P[2] * (cos(pr_tip[5])*sin(pr_tip[3]) - cos(pr_tip[3])*sin(pr_tip[4])*sin(pr_tip[5])) + delta_P[0] * cos(pr_tip[4])*sin(pr_tip[5]);
    p_tool_d[2] = pr_tip[2] - delta_P[0] * sin(pr_tip[4]) + delta_P[2] * cos(pr_tip[3])*cos(pr_tip[4]) + delta_P[1] * cos(pr_tip[4])*sin(pr_tip[3]);
    p_tool_d[3] = atan2(cos(pr_tip[3])*cos(pr_tip[4])*(cos(delta_P[5])*sin(delta_P[3]) + cos(delta_P[3])*sin(delta_P[4])*sin(delta_P[5])) + cos(pr_tip[4])*sin(pr_tip[3])*(cos(delta_P[3])*cos(delta_P[5]) - sin(delta_P[3])*sin(delta_P[4])*sin(delta_P[5])) + cos(delta_P[4])*sin(delta_P[5])*sin(pr_tip[4]), cos(delta_P[3])*cos(delta_P[4])*cos(pr_tip[3])*cos(pr_tip[4]) - sin(delta_P[4])*sin(pr_tip[4]) - cos(delta_P[4])*cos(pr_tip[4])*sin(delta_P[3])*sin(pr_tip[3]));
    p_tool_d[4] = atan2(cos(delta_P[4])*cos(delta_P[5])*sin(pr_tip[4]) - cos(pr_tip[4])*sin(pr_tip[3])*(cos(delta_P[3])*sin(delta_P[5]) + cos(delta_P[5])*sin(delta_P[3])*sin(delta_P[4])) - cos(pr_tip[3])*cos(pr_tip[4])*(sin(delta_P[3])*sin(delta_P[5]) - cos(delta_P[3])*cos(delta_P[5])*sin(delta_P[4])), sqrt(pow(sin(delta_P[4])*sin(pr_tip[4]) - cos(delta_P[3])*cos(delta_P[4])*cos(pr_tip[3])*cos(pr_tip[4]) + cos(delta_P[4])*cos(pr_tip[4])*sin(delta_P[3])*sin(pr_tip[3]), 2) + pow(cos(pr_tip[3])*cos(pr_tip[4])*(cos(delta_P[5])*sin(delta_P[3]) + cos(delta_P[3])*sin(delta_P[4])*sin(delta_P[5])) + cos(pr_tip[4])*sin(pr_tip[3])*(cos(delta_P[3])*cos(delta_P[5]) - sin(delta_P[3])*sin(delta_P[4])*sin(delta_P[5])) + cos(delta_P[4])*sin(delta_P[5])*sin(pr_tip[4]), 2)));
    p_tool_d[5] = atan2((cos(delta_P[3])*sin(delta_P[5]) + cos(delta_P[5])*sin(delta_P[3])*sin(delta_P[4]))*(cos(pr_tip[3])*cos(pr_tip[5]) + sin(pr_tip[3])*sin(pr_tip[4])*sin(pr_tip[5])) - (sin(delta_P[3])*sin(delta_P[5]) - cos(delta_P[3])*cos(delta_P[5])*sin(delta_P[4]))*(cos(pr_tip[5])*sin(pr_tip[3]) - cos(pr_tip[3])*sin(pr_tip[4])*sin(pr_tip[5])) + cos(delta_P[4])*cos(delta_P[5])*cos(pr_tip[4])*sin(pr_tip[5]), (sin(delta_P[3])*sin(delta_P[5]) - cos(delta_P[3])*cos(delta_P[5])*sin(delta_P[4]))*(sin(pr_tip[3])*sin(pr_tip[5]) + cos(pr_tip[3])*cos(pr_tip[5])*sin(pr_tip[4])) - (cos(delta_P[3])*sin(delta_P[5]) + cos(delta_P[5])*sin(delta_P[3])*sin(delta_P[4]))*(cos(pr_tip[3])*sin(pr_tip[5]) - cos(pr_tip[5])*sin(pr_tip[3])*sin(pr_tip[4])) + cos(delta_P[4])*cos(delta_P[5])*cos(pr_tip[4])*cos(pr_tip[5]));
    p_tool_d[0] = round(p_tool_d[0] * 10000.0f) / 10000.0f;
    p_tool_d[1] = round(p_tool_d[1] * 10000.0f) / 10000.0f;
    p_tool_d[2] = round(p_tool_d[2] * 10000.0f) / 10000.0f;
    p_tool_d[3] = round(p_tool_d[3] * 10000.0f) / 10000.0f;
    p_tool_d[4] = round(p_tool_d[4] * 10000.0f) / 10000.0f;
    p_tool_d[5] = round(p_tool_d[5] * 10000.0f) / 10000.0f;
}

//v3
void TPSCAL(const float tg[12], float travelpointseries_data[10][66], float tps_data[10][12], int TPS_size[1])
{

    int n = TPS_size[0];

    float p_delta[6];
    float p_d[6];
    float p_c[6];

    int row_handL_rel2_tgL_data[10];
    int row_handL_rel2_tgL_size = 0;
    int row_handR_rel2_tgR_data[10];
    int row_handR_rel2_tgR_size = 0;

    int row_handL_rel2_handR_data[10];
    int row_handL_rel2_handR_size = 0;
    int row_handR_rel2_handL_data[10];
    int row_handR_rel2_handL_size = 0;

    int row_handL_abs_data[10];
    int row_handL_abs_size = 0;
    int row_handR_abs_data[10];
    int row_handR_abs_size = 0;

    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < 12; j++)
        {
            tps_data[i][j] = 0.0000;
        }
    }

    for (int i = 0; i < n; i++)
    {
        if (MRD(travelpointseries_data[i][0]) == 0 && MRD(travelpointseries_data[i][1]) == 3 && MRD(travelpointseries_data[i][2]) == 2)
        {
            row_handL_rel2_tgL_data[row_handL_rel2_tgL_size] = i;
            row_handL_rel2_tgL_size++;
        }
        if (MRD(travelpointseries_data[i][10]) == 0 && MRD(travelpointseries_data[i][11]) == 3 && MRD(travelpointseries_data[i][12]) == 3)
        {
            row_handR_rel2_tgR_data[row_handR_rel2_tgR_size] = i;
            row_handR_rel2_tgR_size++;
        }
        if (MRD(travelpointseries_data[i][0]) == 0 && MRD(travelpointseries_data[i][1]) == 3 && MRD(travelpointseries_data[i][2]) == 5)
        {
            row_handL_rel2_handR_data[row_handL_rel2_handR_size] = i;
            row_handL_rel2_handR_size++;
        }
        if (MRD(travelpointseries_data[i][10]) == 0 && MRD(travelpointseries_data[i][11]) == 3 && MRD(travelpointseries_data[i][12]) == 4)
        {
            row_handR_rel2_handL_data[row_handR_rel2_handL_size] = i;
            row_handR_rel2_handL_size++;
        }
        if (MRD(travelpointseries_data[i][0]) == 0 && MRD(travelpointseries_data[i][1]) == 1)
        {
            row_handL_abs_data[row_handL_abs_size] = i;
            row_handL_abs_size++;
        }
        if (MRD(travelpointseries_data[i][10]) == 0 && MRD(travelpointseries_data[i][11]) == 1)
        {
            row_handR_abs_data[row_handR_abs_size] = i;
            row_handR_abs_size++;
        }
    }

    //left pos calculation based vision feedback
    if (row_handL_rel2_tgL_size > 0)
    {
        for (int i = 0; i < row_handL_rel2_tgL_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_delta[k] = travelpointseries_data[row_handL_rel2_tgL_data[i]][k + 3];
            }
            for (int k = 0; k < 6; k++)
            {
                p_c[k] = tg[k];
            }
            Rel2Abs(p_delta, p_c, p_d);
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handL_rel2_tgL_data[i]][k] = p_d[k];
            }
            for (int j = 1; j <= row_handL_rel2_tgL_data[i]; j++)
            {
                if (row_handL_rel2_tgL_data[i] > 0 && MRD(travelpointseries_data[row_handL_rel2_tgL_data[i] - j][0]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_rel2_tgL_data[i] - j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_rel2_tgL_data[i] - j + 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_rel2_tgL_data[i] - j][k] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handL_rel2_tgL_data[i] + 1; j++)
            {
                if (row_handL_rel2_tgL_data[i] + j < n && MRD(travelpointseries_data[row_handL_rel2_tgL_data[i] + j][0]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_rel2_tgL_data[i] + j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_rel2_tgL_data[i] + j - 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_rel2_tgL_data[i] + j][k] = p_d[k];
                    }
                }
            }
        }
    }

    //right pos calculation based vision feedback
    if (row_handR_rel2_tgR_size > 0)
    {
        for (int i = 0; i < row_handR_rel2_tgR_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_delta[k] = travelpointseries_data[row_handR_rel2_tgR_data[i]][k + 13];
            }
            for (int k = 0; k < 6; k++)
            {
                p_c[k] = tg[k + 6];
            }
            Rel2Abs(p_delta, p_c, p_d);
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handR_rel2_tgR_data[i]][k + 6] = p_d[k];
            }
            for (int j = 1; j <= row_handR_rel2_tgR_data[i]; j++)
            {
                if (row_handR_rel2_tgR_data[i] > 0 && MRD(travelpointseries_data[row_handR_rel2_tgR_data[i] - j][10]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_rel2_tgR_data[i] - j][k + 13];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_rel2_tgR_data[i] - j + 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_rel2_tgR_data[i] - j][k + 6] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handR_rel2_tgR_data[i] + 1; j++)
            {
                if (row_handR_rel2_tgR_data[i] + j < n && MRD(travelpointseries_data[row_handR_rel2_tgR_data[i] + j][10]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_rel2_tgR_data[i] + j][k + 13];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_rel2_tgR_data[i] + j - 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_rel2_tgR_data[i] + j][k + 6] = p_d[k];
                    }
                }
            }
        }
    }

    // left arm pos calculation based abs coor given
    if (row_handL_abs_size > 0)
    {
        for (int i = 0; i < row_handL_abs_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_d[k] = travelpointseries_data[row_handL_abs_data[i]][k + 3];
            }
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handL_abs_data[i]][k] = p_d[k];
            }
            for (int j = 1; j <= row_handL_abs_data[i]; j++)
            {
                if (row_handL_abs_data[i] > 0 && MRD(travelpointseries_data[row_handL_abs_data[i] - j][0]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_abs_data[i] - j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_abs_data[i] - j + 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);

                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_abs_data[i] - j][k] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handL_abs_data[i] + 1; j++)
            {
                if (row_handL_abs_data[i] + j < n && MRD(travelpointseries_data[row_handL_abs_data[i] + j][0]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_abs_data[i] + j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_abs_data[i] + j - 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);

                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_abs_data[i] + j][k] = p_d[k];
                    }
                }
            }            
        }
    }

    // right arm pos calculation based on abs coor given
    if (row_handR_abs_size > 0)
    {
        for (int i = 0; i < row_handR_abs_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_d[k] = travelpointseries_data[row_handR_abs_data[i]][k + 13];
            }
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handR_abs_data[i]][k + 6] = p_d[k];
            }
            for (int j = 1; j <= row_handR_abs_data[i]; j++)
            {
                if (row_handR_abs_data[i] > 0 && MRD(travelpointseries_data[row_handR_abs_data[i] - j][10]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_abs_data[i] - j][k + 13];

                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_abs_data[i] - j + 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);

                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_abs_data[i] - j][k + 6] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handR_abs_data[i] + 1; j++)
            {
                if (row_handR_abs_data[i] + j < n && MRD(travelpointseries_data[row_handR_abs_data[i] + j][10]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_abs_data[i] + j][k + 13];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_abs_data[i] + j - 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);

                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_abs_data[i] + j][k + 6] = p_d[k];
                    }
                }
            }
        }
    }

    // left arm pos calculation based on right-arm-coor
    if (row_handL_rel2_handR_size > 0)
    {
        for (int i = 0; i < row_handL_rel2_handR_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_delta[k] = travelpointseries_data[row_handL_rel2_handR_data[i]][k + 3];
            }
            for (int k = 0; k < 6; k++)
            {
                p_c[k] = tps_data[row_handL_rel2_handR_data[i]][k + 6];
            }
            Rel2Abs(p_delta, p_c, p_d);
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handL_rel2_handR_data[i]][k] = p_d[k];
            }
            for (int j = 1; j <= row_handL_rel2_handR_data[i]; j++)
            {
                if (row_handL_rel2_handR_data[i] > 0 && MRD(travelpointseries_data[row_handL_rel2_handR_data[i] - j][0]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_rel2_handR_data[i] - j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_rel2_handR_data[i] - j + 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_rel2_handR_data[i] - j][k] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handL_rel2_handR_data[i] + 1; j++)
            {
                if (row_handL_rel2_handR_data[i] + j < n && MRD(travelpointseries_data[row_handL_rel2_handR_data[i] + j][0]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handL_rel2_handR_data[i] + j][k + 3];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handL_rel2_handR_data[i] + j - 1][k];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handL_rel2_handR_data[i] + j][k] = p_d[k];
                    }
                }
            }
        }
    }

    // right arm pos calculation based on left-arm-coor
    if (row_handR_rel2_handL_size > 0)
    {
        for (int i = 0; i < row_handR_rel2_handL_size; i++)
        {
            for (int k = 0; k < 6; k++)
            {
                p_delta[k] = travelpointseries_data[row_handR_rel2_handL_data[i]][k + 13];
            }
            for (int k = 0; k < 6; k++)
            {
                p_c[k] = tps_data[row_handR_rel2_handL_data[i]][k];
            }
            Rel2Abs(p_delta, p_c, p_d);
            for (int k = 0; k < 6; k++)
            {
                tps_data[row_handR_rel2_handL_data[i]][k + 6] = p_d[k];
            }
            for (int j = 1; j <= row_handR_rel2_handL_data[i]; j++)
            {
                if (row_handR_rel2_handL_data[i] > 0 && MRD(travelpointseries_data[row_handR_rel2_handL_data[i] - j][10]) == -1 * j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_rel2_handL_data[i] - j][k + 13];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_rel2_handL_data[i] - j + 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_rel2_handL_data[i] - j][k + 6] = p_d[k];
                    }
                }
            }
            for (int j = 1; j <= n - row_handR_rel2_handL_data[i] + 1; j++)
            {
                if (row_handR_rel2_handL_data[i] + j < n && MRD(travelpointseries_data[row_handR_rel2_handL_data[i] + j][10]) == j)
                {
                    for (int k = 0; k < 6; k++)
                    {
                        p_delta[k] = travelpointseries_data[row_handR_rel2_handL_data[i] + j][k + 13];
                    }
                    for (int k = 0; k < 6; k++)
                    {
                        p_c[k] = tps_data[row_handR_rel2_handL_data[i] + j - 1][k + 6];
                    }
                    Rel2Abs(p_delta, p_c, p_d);
                    for (int k = 0; k < 6; k++)
                    {
                        tps_data[row_handR_rel2_handL_data[i] + j][k + 6] = p_d[k];
                    }
                }
            }
        }
    }
}


void Tool2Arm(const float p_tool_d[6], float xtool, float ytool, float ztool, float p_tip_d[6])
{
    p_tip_d[0] = roundf((p_tool_d[0] - ((ztool * (sin(p_tool_d[3]) * sin(p_tool_d[5]) + cos(p_tool_d[3]) * cos(p_tool_d[5]) * sin(p_tool_d[4])) - ytool * (cos(p_tool_d[3]) * sin(p_tool_d[5]) - cos(p_tool_d[5]) * sin(p_tool_d[3]) * sin(p_tool_d[4]))) + xtool * cos(p_tool_d[4]) * cos(p_tool_d[5]))) * 10000.0f) / 10000.0f;
    p_tip_d[1] = roundf((p_tool_d[1] - ((ytool * (cos(p_tool_d[3]) * cos(p_tool_d[5]) + sin(p_tool_d[3]) * sin(p_tool_d[4]) * sin(p_tool_d[5])) - ztool * (cos(p_tool_d[5]) * sin(p_tool_d[3]) - cos(p_tool_d[3]) * sin(p_tool_d[4]) * sin(p_tool_d[5]))) + xtool * cos(p_tool_d[4]) * sin(p_tool_d[5]))) * 10000.0f) / 10000.0f;
    p_tip_d[2] = roundf((p_tool_d[2] - ((ztool * cos(p_tool_d[3]) * cos(p_tool_d[4]) - xtool * sin(p_tool_d[4])) + ytool * cos(p_tool_d[4]) * sin(p_tool_d[3]))) * 10000.0f) / 10000.0f;
    p_tip_d[3] = p_tool_d[3];
    p_tip_d[4] = p_tool_d[4];
    p_tip_d[5] = p_tool_d[5];
}

void TPS_catchline(float TPS_catchline_data[10][66])
{
    QFile f("TPS_catchline.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_catchline_data[row][col] = x.toFloat();
        }
    }
}

void stepNumber(int stepNumber_data[10][10])
{
    QFile f("stepNumber.csv");
    bool b = f.open(QIODevice::ReadOnly);
    qDebug() << __FUNCTION__ << "  open stepNumber.csv : " << b;
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            stepNumber_data[row][col] = x.toFloat();
        }
    }
}

void TPS_catchline_R(float TPS_catchline_R_data[10][66])
{
    QFile f("TPS_catchline_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_catchline_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_clampToolInstall(float TPS_clampToolInstall_data[10][66])
{
    QFile f("TPS_clampToolInstall.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_clampToolInstall_data[row][col] = x.toFloat();
        }
    }
}

void TPS_clampToolInstall_R(float TPS_clampToolInstall_R_data[10][66])
{
    QFile f("TPS_clampToolInstall_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_clampToolInstall_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_connectline_L(float TPS_connectline_L_data[10][66])
{
    QFile f("TPS_connectline_L.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_connectline_L_data[row][col] = x.toFloat();
        }
    }
}

void TPS_connectline_L2(float TPS_connectline_L2_data[10][66])
{
    QFile f("TPS_connectline_L2.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_connectline_L2_data[row][col] = x.toFloat();
        }
    }
}

void TPS_connectline_R(float TPS_connectline_R_data[10][66])
{
    QFile f("TPS_connectline_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_connectline_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_connectline_R2(float TPS_connectline_R2_data[10][66])
{
    QFile f("TPS_connectline_R2.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_connectline_R2_data[row][col] = x.toFloat();
        }
    }
}

void TPS_disconnectline(float TPS_disconnectline_data[10][66])
{
    QFile f("TPS_disconnectline.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_disconnectline_data[row][col] = x.toFloat();
        }
    }
}

void TPS_disconnectline_R(float TPS_disconnectline_R_data[10][66])
{
    QFile f("TPS_disconnectline_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_disconnectline_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_cutline(float TPS_cutline_data[10][66])
{
    QFile f("TPS_cutline.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_cutline_data[row][col] = x.toFloat();
        }
    }
}

void TPS_cutline_R(float TPS_cutline_R_data[10][66])
{
    QFile f("TPS_cutline_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_cutline_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_addClamp(float TPS_addClamp_data[10][66])
{
    QFile f("TPS_addClamp.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_addClamp_data[row][col] = x.toFloat();
        }
    }
}

void TPS_addClamp_R(float TPS_addClamp_R_data[10][66])
{
    QFile f("TPS_addClamp_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_addClamp_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_rmvClamp(float TPS_rmvClamp_data[10][66])
{
    QFile f("TPS_rmvClamp.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_rmvClamp_data[row][col] = x.toFloat();
        }
    }
}

void TPS_rmvClamp_R(float TPS_rmvClamp_R_data[10][66])
{
    QFile f("TPS_rmvClamp_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_rmvClamp_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_verifLivePart(float TPS_verifLivePart_data[10][66])
{
    QFile f("TPS_verifLivePart.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_verifLivePart_data[row][col] = x.toFloat();
        }
    }
}

void TPS_verifLivePart_R(float TPS_verifLivePart_R_data[10][66])
{
    QFile f("TPS_verifLivePart.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_verifLivePart_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_wireStripping(float TPS_wireStripping_data[10][66])
{
    QFile f("TPS_wireStripping.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_wireStripping_data[row][col] = x.toFloat();
        }
    }
}

void TPS_wireStripping_R(float TPS_wireStripping_R_data[10][66])
{
    QFile f("TPS_wireStripping_R.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_wireStripping_R_data[row][col] = x.toFloat();
        }
    }
}

void TPS_RESET(float TPS_RESET_data[10][66])
{
    QFile f("TPS_reset.csv");
    bool b = f.open(QIODevice::ReadOnly);
    qDebug() << "open TPS_reset: " << b;
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF)
    {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss)
        {
            ++col;
            TPS_RESET_data[row][col] = x.toFloat();
        }
    }
}

void curvefit(float *x, float* y, int n, float p[2])
{
    int i, i1, i2;
    float x2[102400] = { 0 };
    float b_y[4], a[4];
    float r, t;

    for (i = 0; i < n; i++) {
        x2[i] = x[i];
        x2[i + n] = 1.0;
    }

    for (i = 0; i < 2; i++) {
        for (i1 = 0; i1 < 2; i1++) {
            r = 0.0;
            for (i2 = 0; i2 < n; i2++) {
                r += x2[i2 + n * i] * x2[i2 + n * i1];
            }

            b_y[i + (i1 << 1)] = r;
        }
    }

    if (std::abs(b_y[1]) > std::abs(b_y[0])) {
        r = b_y[0] / b_y[1];
        t = 1.0f / (r * b_y[3] - b_y[2]);
        a[0] = b_y[3] / b_y[1] * t;
        a[1] = -t;
        a[2] = -b_y[2] / b_y[1] * t;
        a[3] = r * t;
    }
    else {
        r = b_y[1] / b_y[0];
        t = 1.0f / (b_y[3] - r * b_y[2]);
        a[0] = b_y[3] / b_y[0] * t;
        a[1] = -r * t;
        a[2] = -b_y[2] / b_y[0] * t;
        a[3] = t;
    }

    for (i = 0; i < 2; i++) {
        r = 0.0;
        t = a[i + 2];
        for (i1 = 0; i1 < n; i1++) {
            r += (a[i] * x2[i1] + t * x2[i1 + n]) * y[i1];
        }

        p[i] = r;
    }
}

void evaulatecurvefit(float *x, float* y, float xRange[2], float yRange[2], int n, float p[2], float R[1])
{
    float xDis = (xRange[2] - xRange[1])*0.2f;
    float xTick[6] = {0};
    float xSum[5] = {0};
    float ySum[5] = {0};
    float SumCount[5] = {0};
    float xMean[5] = {0};
    float yMean[5] = {0};
    float yfitted[5] = {0};

    for (int i = 0; i < 6; i++)
    {
        xTick[i] = xRange[0] + xDis * (i);
    }
    for(int i = 0; i<n; i++)
    {
        if(x[i]>xTick[0]&&x[i]<xTick[1])
        {
            xSum[0] = xSum[0] + x[i];
            ySum[0] = ySum[0] + y[i];
            SumCount[0] = SumCount[0]++;
        }
        else if(x[i]>xTick[1]&&x[i]<xTick[2])
        {
            xSum[1] = xSum[1] + x[i];
            ySum[1] = ySum[1] + y[i];
            SumCount[1] = SumCount[1]++;
        }
        else if(x[i]>xTick[2]&&x[i]<xTick[3])
        {
            xSum[2] = xSum[2] + x[i];
            ySum[2] = ySum[2] + y[i];
            SumCount[2] = SumCount[2]++;
        }
        else if(x[i]>xTick[3]&&x[i]<xTick[4])
        {
            xSum[3] = xSum[3] + x[i];
            ySum[3] = ySum[3] + y[i];
            SumCount[3] = SumCount[3]++;
        }
        else if(x[i]>xTick[4]&&x[i]<xTick[5])
        {
            xSum[4] = xSum[4] + x[i];
            ySum[4] = ySum[4] + y[i];
            SumCount[4] = SumCount[4]++;
        }
    }
    for (int i = 0; i < 5; i++)
    {
        if(SumCount[i]>0)
        {
            xMean[i] = xSum[i]/SumCount[i];
            yMean[i] = ySum[i]/SumCount[i];
        }
        else
        {
            xMean[i] = (xTick[i]+xTick[i+1])*0.5f;
            yMean[i] = (yRange[0]+yRange[1])*0.5f;
        }
    }
    for (int i = 0; i < 5; i++)
    {
        yfitted[i] = p[0] * xMean[i] + p[1];
    }
    R[0] = 0;
    for (int i = 0; i < 5; i++)
    {
        R[0] = R[0] + (yfitted[i] - yMean[i])*(yfitted[i] - yMean[i]);
    }
}


void p_r_calaculation(const float Xcurve_z[2], const float Ycurve_z[2], const float Xcurve_y[2], const float Zcurve_y[2], float y_target, float z_target, float p_re2_z[3], float r_re2_z[3], float p_re2_y[3], float r_re2_y[3])
{
    //位置计算
    p_re2_z[0] = Xcurve_z[0] * z_target + Xcurve_z[1];
    p_re2_z[1] = Ycurve_z[0] * z_target + Ycurve_z[1];
    p_re2_z[2] = z_target;

    p_re2_y[0] = Xcurve_y[0] * y_target + Xcurve_y[1];
    p_re2_y[1] = y_target;
    p_re2_y[2] = Zcurve_y[0] * y_target + Zcurve_y[1];

//    //角度计算
//    r_re2_z[0] = atan(-Ycurve_z[0]);
//    qDebug()<<"r_re2_z[0]"<<r_re2_z[0];
//    r_re2_z[1] = Xcurve_z[0];
//    r_re2_z[2] = 0;

//    r_re2_y[0] = atan(Zcurve_y[0]);
//    qDebug()<<"r_re2_y[0]"<<r_re2_y[0];
//    r_re2_y[1] = 0.0;
//    r_re2_y[2] = -Xcurve_y[0];

    //角度计算V2
    r_re2_z[0] = atan(-Ycurve_z[0]/sqrt(Xcurve_z[0] * Xcurve_z[0] + 1));
//    qDebug()<<"r_re2_z[0]"<<r_re2_z[0];
    r_re2_z[1] = atan(Xcurve_z[0]);
    r_re2_z[2] = 0.0f;

    r_re2_y[0] = atan(Zcurve_y[0]/sqrt(Xcurve_y[0] * Xcurve_y[0] + 1));
//    qDebug()<<"r_re2_y[0]"<<r_re2_y[0];
    r_re2_y[1] = 0.0f;
    r_re2_y[2] = atan(-Xcurve_y[0]);
}

void TPSGENR(float tg[30], int Num_mission, float TPS_data[10][22], float toolmass_L_data[10][4], 
float cc_L_data[10][12], float toolmass_R_data[10][4], float cc_R_data[10][12], float ftMode_L[10], 
float ftMode_R[10], float period_L[10][2], float period_R[10][2], int stepNum[1])
{
    float b_tg[12];
    static const signed char iv[12] = { 0, 1, 2, 3, 4, 5, 12, 13, 14, 15, 16, 17 };
    static const signed char iv1[12] = { 6, 7, 8, 9, 10, 11, 18, 19, 20, 21, 22, 23 };
    static const signed char iv2[12] = { 6, 7, 8, 9, 10, 11, 24, 25, 26, 27, 28, 29 };
    static const signed char iv2_2[12] = { 24, 25, 26, 27, 28, 29, 18, 19, 20, 21, 22, 23 };
    static const signed char iv3[12] = { 0, 1, 2, 3, 4, 5, 24, 25, 26, 27, 28, 29 };
    static const signed char iv3_2[12] = { 24, 25, 26, 27, 28, 29, 12, 13, 14, 15, 16, 17 };

    float travelpointseries_data[10][66];

    float tps[10][12];
    float toolSize_L[10][3];
    float toolSize_R[10][3];
    float elbowPos_L[10];
    float elbowPos_R[10];
    float handAction_L[10];
    float handAction_R[10];

    float pose_L[3];
    float lineSize_L[3];
    float pose_R[3];
    float lineSize_R[3];
    //    float lineSize_R[3];

    int stepNumber_data[10][10] = {{0}};
    stepNumber(stepNumber_data);
    int row = floor(Num_mission/10);
    int col = Num_mission - row * 10;
    int SN = stepNumber_data[row][col];
    stepNum[0] = SN;

    if (Num_mission == 1)
    {
        qDebug()<<"Num_mission == 1:TPS_catchline; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 7;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv[i]];
        }

        TPS_catchline(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++) //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 11)
    {
        qDebug()<<"Num_mission == 11:TPS_catchline_R; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 7;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv[i]];
        }

        TPS_catchline_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++) //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 3)
    {
        qDebug()<<"Num_mission == 3:TPS_clampToolInstall; SN = "<<stepNum[0]<<"\n";
//        stepNum[0] = 3;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_clampToolInstall(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 13)
    {
        qDebug()<<"Num_mission == 13:TPS_clampToolInstall_R; SN = "<<stepNum[0]<<"\n";
//        stepNum[0] = 3;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_clampToolInstall_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 4)
    {
        qDebug()<<"Num_mission == 4:TPS_verifLivePart; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 3;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_wireStripping(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 14)
    {
        qDebug()<<"Num_mission == 14:TPS_verifLivePart_R; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 3;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_wireStripping_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 5)
    {
        qDebug()<<"Num_mission == 5:TPS_connectline_L; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 10;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_connectline_L(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
        //-----------------------------------------------
        pose_L[0] = tg[21] - tg[9];
        pose_L[1] = tg[22] - tg[10];
        pose_L[2] = tg[23] - tg[11];
        lineSize_L[0] = tg[6] - 0.6f;
        lineSize_L[1] = tg[7] - 0.3f;
        lineSize_L[2] = tg[8] - 0.2f;

        for (int i = 3; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = lineSize_L[j];
            }
        }
        for (int i = 1; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                tps[i][j + 3] = pose_L[j];
            }
        }
        //-----------------------------------------------
    }
    else if (Num_mission == 25)
    {
        qDebug()<<"Num_mission == 25:TPS_connectline_L2; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 10;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_connectline_L2(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
        //-----------------------------------------------
        pose_L[0] = tg[21] - tg[9];
        pose_L[1] = tg[22] - tg[10];
        pose_L[2] = tg[23] - tg[11];
        lineSize_L[0] = tg[6] - 0.6f;
        lineSize_L[1] = tg[7] - 0.3f;
        lineSize_L[2] = tg[8] - 0.2f;

        for (int i = 3; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = lineSize_L[j];
            }
        }
        for (int i = 1; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                tps[i][j + 3] = pose_L[j];
            }
        }
        //-----------------------------------------------
    }
    else if (Num_mission == 15)
    {
        qDebug()<<"Num_mission == 15:TPS_connectline_R; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 10;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_connectline_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
        //-----------------------------------------------
        pose_R[0] = -tg[21] + tg[9];
        pose_R[1] = -tg[22] + tg[10];
        pose_R[2] = -tg[23] + tg[11];
        lineSize_R[0] = tg[18] - 0.6f;
        lineSize_R[1] = tg[19] + 0.3f;
        lineSize_R[2] = tg[20] - 0.2f;

        for (int i = 3; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_R[i][j] = lineSize_R[j];
            }
        }
        for (int i = 1; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                tps[i][j + 9] = pose_R[j];
            }
        }
        //-----------------------------------------------
    }
    else if (Num_mission == 35)
    {
        qDebug()<<"Num_mission == 35:TPS_connectline_R2; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 10;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv1[i]];
        }

        TPS_connectline_R2(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
        //-----------------------------------------------
        pose_R[0] = -tg[21] + tg[9];
        pose_R[1] = -tg[22] + tg[10];
        pose_R[2] = -tg[23] + tg[11];
        lineSize_R[0] = tg[18] - 0.6f;
        lineSize_R[1] = tg[19] + 0.3f;
        lineSize_R[2] = tg[20] - 0.2f;

        for (int i = 3; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_R[i][j] = lineSize_R[j];
            }
        }
        for (int i = 1; i < (stepNum[0]-1); i++)
        {
            for (int j = 0; j < 3; j++)
            {
                tps[i][j + 9] = pose_R[j];
            }
        }
        //-----------------------------------------------
    }
    else if (Num_mission == 6)
    {
        qDebug()<<"Num_mission == 6:TPS_disconnectline; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 7;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv2[i]];
        }

        TPS_disconnectline(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 16)
    {
        qDebug()<<"Num_mission == 16:TPS_disconnectline_R; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 7;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv2_2[i]];
        }

        TPS_disconnectline_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 36)
    {
        qDebug()<<"Num_mission == 36:TPS_cutline; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 6;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv[i]];
        }

        TPS_cutline(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 46)
    {
        qDebug()<<"Num_mission == 46:TPS_cutline_R; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 6;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = tg[iv[i]];
        }

        TPS_cutline_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize: fill zero first
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 7)
    {
        qDebug()<<"Num_mission == 7:TPS_wireStripping; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 1;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_wireStripping(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 17)
    {
        qDebug()<<"Num_mission == 17:TPS_wireStripping_R; SN ="<<stepNum[0]<<"\n";
        stepNum[0] = 1;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_wireStripping_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 8)
    {
        qDebug()<<"Num_mission == 8:TPS_addClamp; SN = "<<stepNum[0]<<"\n";
//        stepNum[0] = 1;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_addClamp(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 18)
    {
        qDebug()<<"Num_mission == 18:TPS_addClamp_R; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 1;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_addClamp_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 9)
    {
        qDebug()<<"Num_mission == 9:TPS_rmvClamp; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 8;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_rmvClamp(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 19)
    {
        qDebug()<<"Num_mission == 19:TPS_rmvClamp_R; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 8;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_rmvClamp_R(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else if (Num_mission == 0)
    {
        qDebug()<<"Num_mission == 0:TPS_RESET; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 4;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_RESET(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    else
    {
        qDebug()<<"Num_mission == else; SN ="<<stepNum[0]<<"\n";
//        stepNum[0] = 4;

        for (int i = 0; i < 12; i++)
        {
            b_tg[i] = 0;
        }

        TPS_RESET(travelpointseries_data);

        TPSCAL(b_tg, travelpointseries_data, tps, stepNum);

        for (int i = 0; i < stepNum[0]; i++)  //toolsize
        {
            for (int j = 0; j < 3; j++)
            {
                toolSize_L[i][j] = travelpointseries_data[i][22 + j];
                toolSize_R[i][j] = travelpointseries_data[i][25 + j];
            }
        }
    }

    //Tool2Arm trajectory transformating
    for (int i = 0; i < stepNum[0]; i++)
    {
        float tps_tmp_1[6];
        float tps_tmp_2[6];
        float tps_tmp_3[6];
        float tps_tmp_4[6];

        for (int j = 0; j < 6; j++)
        {
            tps_tmp_1[j] = tps[i][j];
            tps_tmp_2[j] = tps[i][j + 6];
        }
        Tool2Arm(tps_tmp_1, toolSize_L[i][0], toolSize_L[i][1], toolSize_L[i][2], tps_tmp_3);
        Tool2Arm(tps_tmp_2, toolSize_R[i][0], toolSize_R[i][1], toolSize_R[i][2], tps_tmp_4);
        for (int j = 0; j < 6; j++)
        {
            tps[i][j] =  tps_tmp_3[j];
            tps[i][j + 6] = tps_tmp_4[j];
        }
    }

    for (int i = 0; i < stepNum[0]; i++)
    {
        elbowPos_L[i] = travelpointseries_data[i][9];
        elbowPos_R[i] = travelpointseries_data[i][19];

        handAction_L[i] = travelpointseries_data[i][20];
        handAction_R[i] = travelpointseries_data[i][21];

        toolmass_L_data[i][0] = travelpointseries_data[i][28];
        toolmass_R_data[i][0] = travelpointseries_data[i][29];

        for (int j = 0; j < 12; j++)
        {
            cc_L_data[i][j] = travelpointseries_data[i][j + 30];
            cc_R_data[i][j] = travelpointseries_data[i][j + 42];
        }

        ftMode_L[i] = travelpointseries_data[i][54];
        ftMode_R[i] = travelpointseries_data[i][55];

        for (int j = 0; j < 3; j++)
        {
            toolmass_L_data[i][1 + j] = travelpointseries_data[i][56 + j];
            toolmass_R_data[i][1 + j] = travelpointseries_data[i][59 + j];
        }

        period_L[i][0] = travelpointseries_data[i][62];
        period_L[i][1] = travelpointseries_data[i][63];
        period_R[i][0] = travelpointseries_data[i][64];
        period_R[i][1] = travelpointseries_data[i][65];

    }

    for (int i = 0; i < stepNum[0]; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            TPS_data[i][j] = tps[i][j];
            TPS_data[i][j + 7] = tps[i][j + 6];
        }
        TPS_data[i][6] = elbowPos_L[i];
        TPS_data[i][13] = elbowPos_R[i];
        TPS_data[i][14] = handAction_L[i];
        TPS_data[i][15] = handAction_R[i];
        for (int j = 0; j < 3; j++)
        {
            TPS_data[i][j + 16] = toolSize_L[i][j];
            TPS_data[i][j + 19] = toolSize_R[i][j];
        }
    }
}
